
#include "Turret.h"


Turret::Turret(InputHandler *input, Camera *camera) {
	m_input = input;
	m_camera = camera;
	m_aimTurret = new Victor(SHOOTER_AIM);
	m_shooterImpulse = new Victor(SHOOTER_IMPULSE);
	m_belts = new Victor(CONVEYOR);
}

void Turret::Aim(float angle) {
	float out = GetOut2(m_input->GetTurretAngle(), angle);
	printf("TurretAngle:%f targ:%f out:%f\n", m_input->GetTurretAngle(), angle, out);
	if (angle < -1 && out < 0) out = 0;
	else if (angle > 1 && out > 0) out = 0;
	//m_aimTurret->Set(out);
	m_aimTurret->Set(m_input->GetDriveSpeed());
}

void Turret::AimWithCamera() {
	Aim(m_camera->GetAngle());
	//m_aimTurret->Set(GetOut2(m_input->GetTurretAngle(), m_camera->GetAngle()));
}

void Turret::AimCameraWithTurret() {
	m_camera->SetServos(m_input->GetTurretAngle(), .5);
}

bool Turret::CanFire() {
	return m_camera->Camera::CanFire();
}

void Turret::FireBall() {
	//TODO: Write real fire code
	m_shooterImpulse->Set(-.7);//-m_input->GetAimTurret());
}

void Turret::UnFireBall() {
	//TODO: Write real fire code
	m_shooterImpulse->Set(0);
}

void Turret::MoveBelts(int direction) {
	if (direction == 1) {
		m_belts->Set(-1);
	} else if (direction == -1) {
		m_belts->Set(1);
	} else {
		m_belts->Set(0);
	}
	//m_belts->Set(m_input->InputHandler::GetLeftJoy());
	//m_belts->Set(direction)
}

float GetOut2(float real, float target) {
	float dif = target - real;
	int mult = -1;
	if (dif < 0) {dif = -dif; mult = 1;}

	printf(" GetOut %f %f %f\n", real, target, dif);
	if (.1 < dif && dif < .3) return .0125 * mult;
	else if (.3 <= dif && dif < .6) return .25 * mult;
	else if (dif >= .6) return .5 * mult;
	else return 0;
}
